/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2009, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/

#include <dwa_local_planner/dwa_planner_ros.h>
#include <Eigen/Core>
#include <cmath>

#include <ros/console.h>

#include <pluginlib/class_list_macros.h>

#include <base_local_planner/goal_functions.h>
#include <nav_msgs/Path.h>

#include <dwa_local_planner/caltime.h>

//register this planner as a BaseLocalPlanner plugin
PLUGINLIB_EXPORT_CLASS(dwa_local_planner::DWAPlannerROS, nav_core::BaseLocalPlanner)
double max_trans_vel;
namespace dwa_local_planner {

  void DWAPlannerROS::reconfigureCB(DWAPlannerConfig &config, uint32_t level) {
      if (setup_ && config.restore_defaults) {
        config = default_config_;
        config.restore_defaults = false;
      }
      if ( ! setup_) {
        default_config_ = config;
        setup_ = true;
      }
      ROS_INFO("dwa_max_vel:%f",config.max_trans_vel);
      // update generic local planner params
      base_local_planner::LocalPlannerLimits limits;
      limits.max_trans_vel = config.max_trans_vel;
      max_trans_vel = limits.max_trans_vel;
      limits.min_trans_vel = config.min_trans_vel;
      limits.max_vel_x = config.max_vel_x;
      limits.min_vel_x = config.min_vel_x;
      limits.max_vel_y = config.max_vel_y;
      limits.min_vel_y = config.min_vel_y;
      limits.max_rot_vel = config.max_rot_vel;
      limits.min_rot_vel = config.min_rot_vel;
      limits.acc_lim_x = config.acc_lim_x;
      limits.acc_lim_y = config.acc_lim_y;
      limits.acc_lim_theta = config.acc_lim_theta;
      limits.acc_limit_trans = config.acc_limit_trans;
      limits.xy_goal_tolerance = config.xy_goal_tolerance;
      limits.yaw_goal_tolerance = config.yaw_goal_tolerance;
      limits.prune_plan = config.prune_plan;
      limits.trans_stopped_vel = config.trans_stopped_vel;
      limits.rot_stopped_vel = config.rot_stopped_vel;
      
      planner_util_.reconfigureCB(limits, config.restore_defaults);

      // update dwa specific configuration
      dp_->reconfigure(config);
  }

  DWAPlannerROS::DWAPlannerROS() : initialized_(false),
      odom_helper_("odom"), setup_(false) {

  }

  void DWAPlannerROS::initialize(
      std::string name,
      tf::TransformListener* tf,
      costmap_2d::Costmap2DROS* costmap_ros) {
    if (! isInitialized()) {

      ros::NodeHandle private_nh("~/" + name);
      g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
      l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
      tf_ = tf;
      costmap_ros_ = costmap_ros;
      costmap_ros_->getRobotPose(current_pose_);

      // make sure to update the costmap we'll use for this cycle
      costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();

      planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID());

      //create the actual planner that we'll use.. it'll configure itself from the parameter server
      dp_ = boost::shared_ptr<DWAPlanner>(new DWAPlanner(name, &planner_util_));

      //add, fix bug
      dp_->setFootprintEarly(costmap_ros_->getRobotFootprint());
      ROS_INFO("foot_print_size:%d",costmap_ros_->getRobotFootprint().size());

      apf_ = boost::shared_ptr<APFPlanner>(new APFPlanner(name, &planner_util_));

      if( private_nh.getParam( "odom_topic", odom_topic_ ))
      {
        odom_helper_.setOdomTopic( odom_topic_ );
      }
      
      initialized_ = true;

      //add 
      int cx = 10, cy = 10;
      p_calc_ = new global_planner::PotentialCalculator(cx, cy);
      anyangle_local_planner_ = new global_planner::AnyAngleExpansion(p_calc_, cx, cy);
      anyangle_local_planner_->setFilledPath();

      dsrv_ = new dynamic_reconfigure::Server<DWAPlannerConfig>(private_nh);
      dynamic_reconfigure::Server<DWAPlannerConfig>::CallbackType cb = boost::bind(&DWAPlannerROS::reconfigureCB, this, _1, _2);
      dsrv_->setCallback(cb);

      int parking_shm_id =1115 ;
      parking_shm.shm_init(parking_shm_id,parking_shm_id<<4);
    }
    else{
      ROS_WARN("This planner has already been initialized, doing nothing.");
    }
  }
  
  bool DWAPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
    if (! isInitialized()) {
      ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
      return false;
    }
    //when we get a new plan, we also want to clear any latch we may have on goal tolerances
    latchedStopRotateController_.resetLatching();

    ROS_INFO("Got new plan");
    return dp_->setPlan(orig_global_plan);
  }

  bool DWAPlannerROS::isGoalReached() {
    if (! isInitialized()) {
      ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
      return false;
    }
    if ( ! costmap_ros_->getRobotPose(current_pose_)) {
      ROS_ERROR("Could not get robot pose");
      return false;
    }

    if(latchedStopRotateController_.isGoalReached(&planner_util_, odom_helper_, current_pose_)) {
      ROS_INFO("Goal reached");
      return true;
    } else {
      return false;
    }
  }

  void DWAPlannerROS::publishLocalPlan(std::vector<geometry_msgs::PoseStamped>& path) {
    base_local_planner::publishPlan(path, l_plan_pub_);
  }


  void DWAPlannerROS::publishGlobalPlan(std::vector<geometry_msgs::PoseStamped>& path) {
    base_local_planner::publishPlan(path, g_plan_pub_);
  }

  DWAPlannerROS::~DWAPlannerROS(){
    //make sure to clean things up
    delete p_calc_;
    delete anyangle_local_planner_;
    delete dsrv_;
  }
  std::vector<std::vector <double> > DWAPlannerROS::getObstacles()
  {
    unsigned int width = planner_util_.getCostmap()->getSizeInCellsX();
    unsigned int height = planner_util_.getCostmap()->getSizeInCellsY();
    unsigned char* costmap_ = planner_util_.getCostmap()->getCharMap();
    unsigned long size = width*height;
    double detect_area = 3.;
    double inner_width = detect_area;
    double inner_height = detect_area;
    unsigned int shift_x = (width - inner_width / planner_util_.getCostmap()->getResolution())/2;
    unsigned int shift_y = (height - inner_height / planner_util_.getCostmap()->getResolution())/2;

    std::vector<std::vector <double> > obs;
    std::vector <double> temp_obs;
    temp_obs.resize(2);
    unsigned int mx,my;
    double wx,wy;
    for(int j = shift_y;j<height-shift_y;j++)
    {
      for(int i = shift_x; i<width-shift_x; i++)
      {
        unsigned long k = j*width + i;
        if(costmap_[k] >= costmap_2d::LETHAL_OBSTACLE)
        { 
          planner_util_.getCostmap()->indexToCells(k,mx,my);
          planner_util_.getCostmap()->mapToWorld(mx,my,wx,wy);        
          temp_obs[0] = wx;
          temp_obs[1] = wy;
          obs.push_back(temp_obs);              
        }
      }
    }
    return obs; 
  }
  bool DWAPlannerROS::isBlocked(tf::Stamped<tf::Pose> &global_pose,\
      geometry_msgs::Twist& cmd_vel,double apf_goal[2])
  {
    if(! isInitialized()){
      ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
      return false;
    }
    Eigen::Vector3f pos(current_pose_.getOrigin().getX(), current_pose_.getOrigin().getY(), \
                        tf::getYaw(current_pose_.getRotation()));
    tf::Stamped<tf::Pose> robot_vel;
    odom_helper_.getRobotVel(robot_vel);

    //compute what trajectory to drive along
    tf::Stamped<tf::Pose> drive_cmds;
    drive_cmds.frame_id_ = costmap_ros_->getBaseFrameID();
    //get obstacle points
    std::vector<std::vector <double> > obstacles = getObstacles();
    double start_point[3] = {pos[0],pos[1],pos[2]};
    apf_->apfInit(start_point,apf_goal,obstacles);
    return false;
    if(obstacles.empty())
    {
      return false;
    }
    
    // call with updated footprint
    bool status = apf_->backRunDetect(robot_vel, drive_cmds);
    //pass along drive commands
    cmd_vel.linear.x = drive_cmds.getOrigin().getX();
    cmd_vel.linear.y = drive_cmds.getOrigin().getY();
    cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());

    return status;
  }
  bool DWAPlannerROS::apfComputeVelocityCommands(tf::Stamped<tf::Pose> &global_pose, \
              geometry_msgs::Twist& cmd_vel, double apf_goal[2]) {
    // dynamic window sampling approach to get useful velocity commands
    if(! isInitialized()){
      ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
      return false;
    }
    Eigen::Vector3f pos(current_pose_.getOrigin().getX(), current_pose_.getOrigin().getY(), \
                        tf::getYaw(current_pose_.getRotation()));
    tf::Stamped<tf::Pose> robot_vel;
    odom_helper_.getRobotVel(robot_vel);

    //compute what trajectory to drive along
    tf::Stamped<tf::Pose> drive_cmds;
    drive_cmds.frame_id_ = costmap_ros_->getBaseFrameID();
    //get obstacle points
    std::vector<std::vector <double> > obstacles = getObstacles();
    double start_point[3] = {pos[0],pos[1],pos[3]};

    apf_->apfInit(start_point,apf_goal,obstacles);
    // call with updated footprint
    apf_->findBestPath(global_pose, robot_vel, drive_cmds,costmap_ros_->getRobotFootprint());
    //pass along drive commands
    cmd_vel.linear.x = drive_cmds.getOrigin().getX();
    cmd_vel.linear.y = drive_cmds.getOrigin().getY();
    cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());

    return true;
  }

  bool DWAPlannerROS::apfParking(tf::Stamped<tf::Pose> &global_pose, \
              geometry_msgs::Twist& cmd_vel, double apf_goal[2]) {
    // dynamic window sampling approach to get useful velocity commands
    if(! isInitialized()){
      ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
      return false;
    }
    Eigen::Vector3f pos(current_pose_.getOrigin().getX(), current_pose_.getOrigin().getY(), \
                        tf::getYaw(current_pose_.getRotation()));
    tf::Stamped<tf::Pose> robot_vel;
    odom_helper_.getRobotVel(robot_vel);

    //compute what trajectory to drive along
    tf::Stamped<tf::Pose> drive_cmds;
    drive_cmds.frame_id_ = costmap_ros_->getBaseFrameID();
    //get obstacle points
//    std::vector<std::vector <double> > obstacles = getObstacles();
//    double start_point[3] = {pos[0],pos[1],pos[3]};

//    apf_->apfInit(start_point,apf_goal,obstacles);
    // call with updated footprint
    apf_->parking(global_pose, robot_vel, drive_cmds,costmap_ros_->getRobotFootprint());
    //pass along drive commands
    cmd_vel.linear.x = drive_cmds.getOrigin().getX();
    cmd_vel.linear.y = drive_cmds.getOrigin().getY();
    cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());

    return true;
  }

  bool DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped<tf::Pose> &global_pose, geometry_msgs::Twist& cmd_vel) {
    // dynamic window sampling approach to get useful velocity commands
    if(! isInitialized()){
      ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
      return false;
    }

    if(!DWAPlan_)
      return false;

    tf::Stamped<tf::Pose> robot_vel;
    odom_helper_.getRobotVel(robot_vel);

    //compute what trajectory to drive along
    tf::Stamped<tf::Pose> drive_cmds;
    drive_cmds.frame_id_ = costmap_ros_->getBaseFrameID();
    
    // call with updated footprint
    base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds, costmap_ros_->getRobotFootprint());
    //ROS_ERROR("Best: %.2f, %.2f, %.2f, %.2f", path.xv_, path.yv_, path.thetav_, path.cost_);

    //pass along drive commands
    cmd_vel.linear.x = drive_cmds.getOrigin().getX();
    cmd_vel.linear.y = drive_cmds.getOrigin().getY();
    cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());

    //if we cannot move... tell someone
    std::vector<geometry_msgs::PoseStamped> local_plan;
    if(path.cost_ < 0) {
      ROS_INFO("The dwa local planner failed to find a valid plan, cost functions discarded all candidates. This can mean there is an obstacle too close to the robot.");
      local_plan.clear();
      publishLocalPlan(local_plan);
      return false;
    }    
    
    //ROS_INFO("dwa_local_planner", "A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.",cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
    
    // Fill out the local plan
    for(unsigned int i = 0; i < path.getPointsSize(); ++i) {
      double p_x, p_y, p_th;
      path.getPoint(i, p_x, p_y, p_th);

      tf::Stamped<tf::Pose> p =
              tf::Stamped<tf::Pose>(tf::Pose(
                      tf::createQuaternionFromYaw(p_th),
                      tf::Point(p_x, p_y, 0.0)),
                      ros::Time::now(),
                      costmap_ros_->getGlobalFrameID());
      geometry_msgs::PoseStamped pose;
      tf::poseStampedTFToMsg(p, pose);
      local_plan.push_back(pose);
    }
    
    //publish information to the visualizer
    publishLocalPlan(local_plan);
    return true;
  }

  void DWAPlannerROS::setSpeed(double vx, double vth, geometry_msgs::Twist& cmd_vel)
  {
    cmd_vel.linear.x = vx;
    cmd_vel.angular.z = vth;
  }
  void DWAPlannerROS::clearRobot(float size_x, float size_y)
  {
    //clear the controller's costmap    
    std::vector<geometry_msgs::Point> clear_poly;
    geometry_msgs::Point pt;
    float x = current_pose_.getOrigin().x();
    float y = current_pose_.getOrigin().y();

    pt.x = x - size_x / 2;
    pt.y = y - size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x + size_x / 2;
    pt.y = y - size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x + size_x / 2;
    pt.y = y + size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x - size_x / 2;
    pt.y = y + size_y / 2;
    clear_poly.push_back(pt);

    planner_util_.getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
  }
  
  bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {
    // dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal
    //Caltime t;
    //t.set_start_time();

    if( ! costmap_ros_->getRobotPose(current_pose_)) {
      ROS_ERROR("Could not get robot pose");
      return false;
    }
    std::vector<geometry_msgs::PoseStamped> transformed_plan;
    if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {
      ROS_ERROR("Could not get local plan");
      return false;
    }
    //if the global plan passed in is empty... we won't do anything
    if(transformed_plan.empty()) {
      ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");
      return false;
    }
       
    Eigen::Vector3f pos(current_pose_.getOrigin().getX(), current_pose_.getOrigin().getY(), tf::getYaw(current_pose_.getRotation()));
    unsigned int cell_start_x,cell_start_y;
    unsigned int cell_goal_x,cell_goal_y;
    int size = transformed_plan.size();
    if(size == 0)
    {
      ROS_WARN("no local path data");
      return false;
    }
    double apf_goal[2] = {transformed_plan[size-1].pose.position.x,
                          transformed_plan[size-1].pose.position.y};

    double goal[2];
    bool goal_clear = false;

    if(!planner_util_.getCostmap()->worldToMap(pos[0], pos[1], cell_start_x, cell_start_y)){
      ROS_ERROR("start point exceed local map ...");
      return false;
    }

    unsigned char start_value = planner_util_.getCostmap()->getCost(cell_start_x, cell_start_y);
    int cnt_local_goal = 0;
    for(int i=0;i<size;i++){
      goal[0] = transformed_plan[size-1-i].pose.position.x;
      goal[1] = transformed_plan[size-1-i].pose.position.y;

      if(!planner_util_.getCostmap()->worldToMap(goal[0], goal[1], cell_goal_x, cell_goal_y)){
        //ROS_WARN("stop point exceed local map ...");
        continue;
      }
      if(costmap_2d::FREE_SPACE == planner_util_.getCostmap()->getCost(cell_goal_x, cell_goal_y)){
        goal_clear = true;
        cnt_local_goal = size-1-i;
        break;
      }
    }

    if(isBlocked(current_pose_, cmd_vel, apf_goal))
    {
      return true;
    }

    /******************parking*****************/
    if(parking_shm.shm_read() == "parking")
    {
        if(latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)) {
        //publish an empty plan because we've reached our goal position
        std::vector<geometry_msgs::PoseStamped> local_plan;
        std::vector<geometry_msgs::PoseStamped> transformed_plan;
        publishGlobalPlan(transformed_plan);
        publishLocalPlan(local_plan);
        base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits();
        return latchedStopRotateController_.computeVelocityCommandsStopRotate(
            cmd_vel,
            limits.getAccLimits(),
            dp_->getSimPeriod(),
            &planner_util_,
            odom_helper_,
            current_pose_,
            boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3));
        }
        else{
          bool status = apfParking(current_pose_, cmd_vel, apf_goal);
          return status;
        }
    }
    
    //ROS_INFO("current_pos:(%f,%f),apf_goal:(%f,%f),dwa_goal:(%f,%f)",pos[0],pos[1],apf_goal[0],apf_goal[1],goal[0],goal[1]);
    int nx  = planner_util_.getCostmap()->getSizeInCellsX();
    int ny = planner_util_.getCostmap()->getSizeInCellsY(); 
    anyangle_local_planner_->setSize(nx,ny);

    unsigned char goal_value = planner_util_.getCostmap()->getCost(cell_goal_x, cell_goal_y);
    unsigned char threshold_start_value = 50;
    std::vector<std::vector<unsigned int> > path;
    
    clearRobot(0.25,0.25);
    if(costmap_2d::FREE_SPACE == goal_value )//&& max_trans_vel > 1.5)
    {
      try{
        bool found_legal = anyangle_local_planner_->path_find(planner_util_.getCostmap()->getCharMap(), 
        planner_util_.getCostmap()->getResolution(), cell_start_x, cell_start_y, cell_goal_x, cell_goal_y, &path);
      }
      catch(...){}
    }
    DWAPlan_ = true;
    int path_size = path.size();
    transformed_plan.clear();

    if(path_size>0){
      geometry_msgs::PoseStamped poseTemp;
      poseTemp.header = transformed_plan[cnt_local_goal].header;
      poseTemp.pose.orientation = transformed_plan[size-1].pose.orientation;     
      std::vector<geometry_msgs::PoseStamped>().swap(transformed_plan);
      transformed_plan.resize(path_size);
      for(int i=0;i<path_size;i++)
      {
        double pos_temp[2];
        planner_util_.getCostmap()->mapToWorld(path[i][0],path[i][1],pos_temp[0],pos_temp[1]);
        transformed_plan[i].header = poseTemp.header;
        transformed_plan[i].pose.position.x = pos_temp[0];
        transformed_plan[i].pose.position.y = pos_temp[1];
        transformed_plan[i].pose.position.z = 0.;
        transformed_plan[i].pose.orientation = poseTemp.pose.orientation;
      }
    }
    else{
      DWAPlan_ = false;
      ROS_WARN("transformed_plan size is zero, can not find local plan...");
      
      tf::Stamped<tf::Pose> robot_vel;
      odom_helper_.getRobotVel(robot_vel);

      tf::Stamped<tf::Pose> drive_cmds;
      drive_cmds.frame_id_ = costmap_ros_->getBaseFrameID();
      apf_->setSpeedSmooth(0.,0.,robot_vel,drive_cmds);

      cmd_vel.linear.x = drive_cmds.getOrigin().getX();
      cmd_vel.linear.y = drive_cmds.getOrigin().getY();
      cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());
      return true;
    }
    
    if(path_size>5)
    {
      publishGlobalPlan(transformed_plan);
      double angle; 
      double aver_angle = 0;
      for(int i=3;i<5;i++)
      {
        double temp_angle = atan2(transformed_plan[i].pose.position.y-pos[1], transformed_plan[i].pose.position.x-pos[0]) - pos[2];
        while(temp_angle>  M_PI) temp_angle -= 2*M_PI;
        while(temp_angle<=-M_PI) temp_angle += 2*M_PI;
        aver_angle += temp_angle;
      }
      aver_angle = aver_angle/3;
      if(fabs(aver_angle)>M_PI/3)
      {
        double vth = 0.4;
        cmd_vel.linear.x = 0.;
        cmd_vel.linear.y = 0.;
        if(aver_angle>0.)
          cmd_vel.angular.z = vth;
        else 
          cmd_vel.angular.z = -vth;
        
        return true;
      }
    }
    //ROS_INFO("transformed_plan:%d",transformed_plan.size());
    // update plan in dwa_planner even if we just stop and rotate, to allow checkTrajectory
    if(DWAPlan_)
        dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan);

    if(DWAPlan_ && latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)) {
      //publish an empty plan because we've reached our goal position
      std::vector<geometry_msgs::PoseStamped> local_plan;
      std::vector<geometry_msgs::PoseStamped> transformed_plan;
      publishGlobalPlan(transformed_plan);
      publishLocalPlan(local_plan);
      base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits();
      return latchedStopRotateController_.computeVelocityCommandsStopRotate(
          cmd_vel,
          limits.getAccLimits(),
          dp_->getSimPeriod(),
          &planner_util_,
          odom_helper_,
          current_pose_,
          boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3));
    } else {
      bool isOk = false;
      if(DWAPlan_)
          isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel);
      if (isOk) {
        publishGlobalPlan(transformed_plan);
        //t.set_stop_time();
        //ROS_INFO("control time use:%f",t.cal_time_ms());
        return true;
      } else {
        ROS_WARN("apf planner recovery");
        bool status = apfComputeVelocityCommands(current_pose_, cmd_vel, apf_goal);
        //t.set_stop_time();
        //ROS_INFO("control time use:%f",t.cal_time_ms());
        return status;
      }      
    }
  }
};
